import sys,os
curPath = os.path.abspath(os.path.dirname(__file__))
rootPath = os.path.split(curPath)[0]
sys.path.append(os.path.split(rootPath)[0])
from ftauto.scservo_sdk import *
from ftauto import ping
from ftauto import forwTorture
from flask import jsonify
import cv2 as cv

def pingID(n):
    return ping.scanID(ID=n)    

#确定哪个舵机行动
def act(id,fvel,facc,fdis,flag):     
    #驱动舵机动作
    sva = forwTorture.servoAction(id)  
    sva.forwardUp(fvel,facc,fdis,flag)
    return jsonify({"status":666,"id":id}) 

def stop(id):
    sva = forwTorture.servoAction(id)
    sva.stop()
    present_position=sva.getLoc()
    return jsonify({"status":200,"id":id,"current_position":present_position}) 
    
def getCurrentLocation(id):
    #实例化执行类 
    sva = forwTorture.servoAction(id)    
    present_position=sva.getLoc()
    return jsonify({"status":200,"id":id,"current_position":present_position})     

# 找到目标函数
def find_marker(image):
    # convert the image to grayscale, blur it, and detect edges
    #将图像转换成灰度、模糊和检测边缘
    gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)  
    gray = cv.GaussianBlur(gray, (5, 5), 0)        
    edged = cv.Canny(gray, 35, 125)               
 
    # find the contours in the edged image and keep the largest one;
    #在边缘图像中找到轮廓并保持最大的轮廓
    # we'll assume that this is our piece of paper in the image
    #我们假设这是我们在图像中的一张纸
    (cnts, _) = cv.findContours(edged.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    # 求最大面积
    
    c = max(cnts, key = cv.contourArea)
 
    # compute the bounding box of the of the paper region and return it
    #计算纸张区域的边界框并返回它
    # cv2.minAreaRect() c代表点集，返回rect[0]是最小外接矩形中心点坐标，
    # rect[1][0]是width，rect[1][1]是height，rect[2]是角度
    return cv.minAreaRect(c)

# 距离计算函数 
def distance_to_camera(knownWidth, perWidth):  
    # compute and return the distance from the maker to the camera
    #计算并返回从目标到相机的距离
    return (knownWidth * 8) / perWidth  


#计算焦距
def calculate_focalDistance(img):       
    marker = find_marker(img)
    # 得到最小外接矩形的中心点坐标，长宽，旋转角度
    # 其中marker[1][0]是该矩形的宽度，单位为像素
 
    focalLength = (marker[1][0] * 15) / 15
    # 获取摄像头的焦距
 
    return focalLength
 